% 初始化参数
A = eye(3); % 系统动态矩阵，这里假设状态是三维的，即两个传感器的状态加上一个共同的状态
H = eye(3); % 观测矩阵，这里假设两个传感器的观测值可以直接组合
Q = 0.01 * eye(3); % 过程噪声协方差矩阵
R = 0.1 * eye(3); % 观测噪声协方差矩阵，这里假设两个传感器的观测噪声是独立的
x_est = [0; 0; 0]; % 初始状态估计，这里假设初始状态是0
P_est = 1000 * eye(3); % 初始误差协方差矩阵

% 模拟一些观测数据
true_distance = 10; % 真实距离
measurements = [true_distance + randn * sqrt(R(1,1)); true_distance+5+ randn * sqrt(R(2,2))]; % 带有噪声的观测数据

% 卡尔曼滤波过程
for i = 1:size(measurements, 1)
    % 预测步骤
    x_pred = A * x_est; % 预测下一个状态
    P_pred = A * P_est * A' + Q; % 预测误差协方差
    
    % 更新步骤
    K = P_pred * H' / (H * P_pred * H' + R); % 计算卡尔曼增益
    x_est = x_pred + K * (measurements(i) - H * x_pred); % 更新状态估计
    P_est = (eye(size(A)) - K * H) * P_pred; % 更新误差协方差
end

% 显示结果
figure;
plot(measurements, 'b');
hold on;
plot(x_est(1:2, end), 'r--'); % 绘制最终的状态估计
legend('Noisy Measurements', 'Kalman Filtered Estimates');
title('Kalman Filtered Distance Measurements with Dual Sensors');
xlabel('Time Step');
ylabel('Distance');